
#include "../include/planner.hpp"

#include <cstdio>

Planner::Planner(/* args */)
{   
    this->qp_solver = new QPSolver(false);
}

Planner::~Planner()
{
}


bool Planner::getParams() {
    if (!ros::param::get("plan_dis", this->plan_dis))   return false;
    if (!ros::param::get("interval", this->interval))   return false;
    /***************************** 动态规划规划参数 ***********************************/
    if (!ros::param::get("delta_s", this->delta_s))     return false;
    if (!ros::param::get("delta_l", this->delta_l))     return false;
    if (!ros::param::get("dp_row_num", this->dp_row_num))   return false;
    if (!ros::param::get("stitch_num", this->stitch_num))   return false;
    if (!ros::param::get("W_cost_ref", this->W_cost_ref))   return false;
    if (!ros::param::get("W_cost_collision", this->W_cost_collision))   return false;
    if (!ros::param::get("W_alpha_collision", this->W_alpha_collision))   return false;
    // 动态规划平滑参数
    if (!ros::param::get("W_cost_smooth_dl", this->W_cost_smooth_dl))   return false;
    if (!ros::param::get("W_cost_smooth_ddl", this->W_cost_smooth_ddl))   return false;
    if (!ros::param::get("W_cost_smooth_dddl", this->W_cost_smooth_dddl))   return false;
    /***************************** 二次规划参数 ***********************************/
    if (!ros::param::get("qp_cost_l", this->qp_cost_l))   return false;
    if (!ros::param::get("qp_cost_dl", this->qp_cost_dl))   return false;
    if (!ros::param::get("qp_cost_ddl", this->qp_cost_ddl))   return false;
    if (!ros::param::get("qp_cost_dddl", this->qp_cost_dddl))   return false;
    if (!ros::param::get("qp_cost_center", this->qp_cost_center))   return false;
    if (!ros::param::get("qp_cost_end_l", this->qp_cost_end_l))   return false;
    if (!ros::param::get("qp_cost_end_dl", this->qp_cost_end_dl))   return false;
    if (!ros::param::get("qp_cost_end_ddl", this->qp_cost_end_ddl))   return false;
    this->dp_col_num = this->plan_dis / this->delta_s;
    return true;
}


/**
 * 规划器主流程
 * 步骤1：确定规划起点
 * 步骤2：将规划起点和障碍物信息转换到参考线上
*/
bool Planner::runPlanner(vector<TraPoint> &trajectory) {
    // 更新参数设置
    if (!this->getParams()) {
        ROS_WARN_ONCE("runPlanner: Read params faild!!!");
        return false;
    }
    // 获取当前参考线
    vector<ReferencePoint> cur_ref_line(this->referenceline_provider.getReferenceLine());
    // 当前车辆定位坐标
    Location host(this->host_info.getCurrentLocation());
    // 障碍物坐标
    vector<Location> obs_vec = this->obs_info.getObstacle(host);

    if (cur_ref_line.size() == 0) {
        ROS_WARN_ONCE("runPlanner: not get reference_line !!1");
        return false;
    }

    if (host.heading == 0) {
        ROS_WARN_ONCE("runPlanner: host.heading == 0 !!1");
        return false;
    }
    
    /**
     * 步骤1：计算规划起点
    */
    // 上次规划周期生成的轨迹
    vector<TraPoint> pre_tra(this->pre_trajectory);
    
    Location plan_start;                // 本周起的规划起点坐标
    vector<TraPoint> stitch_tra;        // 需要拼接的轨迹
    if (!this->calc_plan_start(pre_tra, host, plan_start, stitch_tra)) {
        ROS_WARN_ONCE("calc_plan_start: get some wrong!!");
        return false;
    }
    // cout << "stitch_tra.size: " << stitch_tra.size() << endl;
    /**
     * 步骤2：将规划起点坐标投影到以参考线为轨迹的Frenet坐标系
    */
    SLPoint plan_start_sl;
    ReferencePoint plan_start_proj;
    int plan_start_m_index;
    if (!this->referenceline_provider.calculateProjection(plan_start.x, plan_start.y, 
                                cur_ref_line, plan_start_m_index, plan_start_proj)) {
        ROS_WARN_ONCE("XY2SL: plan_start to SL get some wrong !!!");
        return false;
    }
    this->referenceline_provider.cartesian2Frenet(plan_start_proj, plan_start, plan_start_sl);

    // 如果剩余参考线长度不够规划的长度，则退出规划
    if (cur_ref_line.at(cur_ref_line.size() - 1).s - plan_start_proj.s < 60) {
        ROS_WARN_ONCE("Reference_line length is less than plan trajectory length");
        return false;
    }
    
    // std::cout << "plan_start_s: " << plan_start_sl.s << " plan_start_l: " << plan_start_sl.l << std::endl;
    /**
     * 步骤3：将按障碍物投影到以参考线为轨迹的Frenet坐标系
    */
    vector<SLPoint> sl_obs_vec;
    
    for (int i = 0; i < obs_vec.size(); i++) {
        sl_obs_vec.push_back(SLPoint());
        if (!this->referenceline_provider.XY2SL(obs_vec[i], cur_ref_line, sl_obs_vec[i])) {
            ROS_WARN_ONCE("XY2SL: obstacle to SL get some wrong !!!");
            return false;
        }
    }
    // std::cout << "obs_vec.size: " << obs_vec.size() << " sl_obs_vec.size: " << sl_obs_vec.size() << std::endl;
    /**
     * 步骤4：动态规划，开辟凸空间（粗解）
    */
    vector<SLPoint> plan_tra_sl;
    this->dp_decision(plan_start, plan_start_sl, plan_start_proj, plan_start_m_index, 
                        cur_ref_line, sl_obs_vec, plan_tra_sl);

    /**
     * 步骤5：二次规划
    */
    if (sl_obs_vec.size() > 0) {
        this->qp_optimal(plan_start_sl, sl_obs_vec, plan_tra_sl);
    }

   /**
    * 步骤6：将规划轨迹转化为笛卡尔坐标系，对轨迹进行拼接，并添加时间信息
   */

    vector<Location> plan_tra;
    this->traSL2XY(plan_start_proj, plan_start_m_index, cur_ref_line, plan_tra_sl, plan_tra);

    // 速度规划
    this->s_planner = new SpeedPlanner(plan_start, plan_start_sl, plan_tra, plan_tra_sl);
    vector<STMap> st_vec;
    this->s_planner->genSTMap(obs_vec, sl_obs_vec, st_vec);
    vector<double> s_vec, t_vec, v_vec;
    this->s_planner->dp_decision(st_vec, s_vec, v_vec, t_vec);
    free(this->s_planner);

    uint32_t cur_time = this->rosTime2MSec(ros::Time::now());

    /* 发布ST图消息 */
    em_planner::SpeedPlannerInfo sp_msg;
    if (st_vec.size() == 1) {
        sp_msg.obs_s_vec.push_back(st_vec[0].s_in);
        sp_msg.obs_s_vec.push_back(st_vec[0].s_out);
        sp_msg.obs_t_vec.push_back(st_vec[0].t_in);
        sp_msg.obs_t_vec.push_back(st_vec[0].t_out);
    }
    sp_msg.s_vec.assign(s_vec.begin(), s_vec.end());
    sp_msg.v_vec.assign(v_vec.begin(), v_vec.end());
    sp_msg.t_vec.assign(t_vec.begin(), t_vec.end());
    this->speed_planner_info_pub.publish(sp_msg);

    /* 打印规划速度信息 */
    // for (int i = 0; i < v_vec.size(); i++) {
    //     std::cout << v_vec[i] << " ";
    // }
    // std::cout << std::endl;
    // for (int i = 0; i < s_vec.size(); i++) {
    //     std::cout << s_vec[i] << " ";
    // }
    // std::cout << std::endl;
    
    
    // int len_stitch_tra = stitch_tra.size();
    // if (len_stitch_tra > 0) {
    //     cur_time -= len_stitch_tra * 167;
    //     // 将 stitch_tra 拼接到轨迹前面
    //     // trajectory.assign(stitch_tra.begin(), stitch_tra.end());
    //     for (int i = 0; i < len_stitch_tra; i++) {
    //         stitch_tra[i].time = cur_time;
    //         trajectory.push_back(stitch_tra[i]);
    //         cur_time += 167;
    //     }
    // }
    trajectory.assign(stitch_tra.begin(), stitch_tra.end());
    int speed_plan_len = v_vec.size();
    for (int i = 0; i < plan_tra.size(); i++) {
        TraPoint tra_point;
        tra_point.x = plan_tra[i].x;
        tra_point.y = plan_tra[i].y;
        tra_point.heading = plan_tra[i].heading;
        tra_point.kappa = plan_tra[i].kappa;
        tra_point.accel = 0.0;
        if (i < speed_plan_len) {
            tra_point.speed = v_vec[i];
            tra_point.time = cur_time;
            cur_time += uint32_t(t_vec[i] * 1000);
        } else {
            tra_point.speed = 3.0;
            tra_point.time = cur_time;
            cur_time += 167;
        }
        
        trajectory.push_back(tra_point);
    }
    return true;
}


bool Planner::qp_optimal(const SLPoint &plan_start_sl, 
                         const vector<SLPoint> &obs_vec_sl,
                         vector<SLPoint> &plan_tra_sl)
{
    // 计算 l_min 和 l_max
    int length = plan_tra_sl.size();
    vector<double> l_min(length, -6);
    vector<double> l_max(length, 6);
    this->getLBoundary(plan_tra_sl, obs_vec_sl, l_min, l_max);
    // plt::clf();
    // plt::plot(l_min, "r");
    // plt::plot(l_max, "y");
    // plt::grid(true);
    // plt::pause(0.0001);

    double host_d1 = 2.5;
    double host_d2 = 2.5;
    double host_width = 1.5;
    vector<double> path_l, path_dl, path_ddl;
    this->qp_solver->initJerkMat(l_min, l_max, host_d1, host_d2, host_width, plan_start_sl, 
                                 qp_cost_l, qp_cost_dl, qp_cost_ddl, qp_cost_dddl, 
                                 qp_cost_center, qp_cost_end_l, qp_cost_end_dl, qp_cost_end_ddl);

    if (!this->qp_solver->runJark(path_l, path_dl, path_ddl)) {
        ROS_WARN_ONCE("qp_optimal: OSQP meet some wrong !!!");
    } else {
        for (int i = 0; i < plan_tra_sl.size(); i++) {
            plan_tra_sl[i].l = path_l[i];
            plan_tra_sl[i].l_prime = path_dl[i];
            plan_tra_sl[i].l_dprime = path_ddl[i];
        }
    }
    return true;
}


/**
 * 该函数计算二次优化的空间 l_min 和 l_max 上下界
*/
bool Planner::getLBoundary(const vector<SLPoint> &dp_path_sl, const vector<SLPoint> &obs_vec_sl, 
                  vector<double> &l_min, vector<double> &l_max)
{
    int length = l_min.size();
    vector<double> base_line(length, 0);
    double obs_length = 3.0;
    // 遍历障碍物
    for (int i = 0; i < obs_vec_sl.size(); i++) {
        // 计算障碍物的头部和尾部
        double obs_s_min = obs_vec_sl[i].s - obs_length / 2.0;
        double obs_s_max = obs_vec_sl[i].s + obs_length / 2.0;
        // 找到 obs_s_min obs_s_max 在 dp_path 中的 index
        int start_index = getNearestIndex(dp_path_sl, obs_s_min);
        int end_index = getNearestIndex(dp_path_sl, obs_s_max);
        // 判断 dp_path_l 在障碍物的上面还是下面（左边绕还是右边绕）
        int center_index = getNearestIndex(dp_path_sl, obs_vec_sl[i].s);
        double path_l = dp_path_sl[center_index].l;
        if (start_index == 0 && end_index == 0) {
            // 障碍物已经在小车的后面
            continue;
        }
        // 车辆安全冗余长度
        int redundancy = 3;
        start_index = max(0, start_index - redundancy);
        end_index = min(length - 1, end_index + redundancy);
        if (path_l > obs_vec_sl[i].l) {
            // 向左绕行，凸边界在障碍物的上方，找车子行驶的下边界，也就是寻找 l_min 的最大值
            // l_min.assign(base_line.begin(), base_line.end());
            for (int j = start_index; j < end_index; j++) {
                // l_min(j) 是所有决策为向左绕过障碍物的l下边界的最大值
                l_min[j] = max(l_min[j], obs_vec_sl[i].l + obs_length / 2.0);
            }
        } else {
            // 向右绕行，凸边界在障碍物的下方，找车子行驶的上边界，也就是寻找 l_max 的最小值
            // l_max.assign(base_line.begin(), base_line.end());
            for (int j = start_index; j < end_index; j++) {
                // l_max(j) 是所有决策为向右绕过障碍物的l上边界的最小值
                l_max[j] = min(l_max[j], obs_vec_sl[i].l -  obs_length / 2.0);
            }
        }
    }
    return true;
}


/**
 * 返回 dp_path_s 中与 obs 最近的点的下标
 * 这里的 dp_path_sl 是动态规划算法的规划结果，所以s必然是大于 0 的
 * 而 obs_s 参考线上的点，有可能会小于零
*/
int Planner::getNearestIndex(const vector<SLPoint> &dp_path_sl, const double &obs_sl_s)
{
    if (dp_path_sl.begin()->s > obs_sl_s) {
        // 障碍物在规划 dp_path_sl 的前面
        return 0;
    } else if ((dp_path_sl.end() - 1)->s < obs_sl_s) {
        // 障碍物在 dp_path_sl 的后面，离得还很远
        return dp_path_sl.size() - 1;
    } else {
        int index = 0;
        while (dp_path_sl[index].s < obs_sl_s)
        {
            index++;
        }
        // 还要 判断 index 和 index - 1 那个点距离 obs_sl_s 最近
        if (dp_path_sl[index].s - obs_sl_s < obs_sl_s - dp_path_sl[index-1].s)
            return index;
        else
            return index - 1;   
    }
}


/**
 * 获取规划起点信息
 * 根据上周期规划的轨迹和当前小车位置，计算当前周期规划起点以及需要拼接的轨迹
 * @param pre_plan_tra: 上周期规划的轨迹
 * @param host: 当前车辆的位置
 * @return plan_start: 规划起点信息
 * @return stitch_tra: 待拼接轨迹，一般是上一周期的 current_time + 100ms位置往前取 10个点
*/
bool Planner::calc_plan_start(vector<TraPoint> &pre_plan_tra, Location host, 
                        Location &plan_start, vector<TraPoint> &stitch_tra) {
    // int stitch_num = 10;                 // 需要拼接的轨迹长度
    double dt = 0.1;    // 规划周期 100ms，这里单位为秒
    uint32_t cur_time = this->rosTime2MSec(ros::Time::now());
    if (pre_plan_tra.size() == 0) {
        // 首次运行规划，此时车辆速度为 0，没必要使用车辆动力学递推，直接用定位信息
        plan_start.x = host.x;
        plan_start.y = host.y;
        plan_start.heading = host.heading;
        plan_start.kappa = host.kappa;
        plan_start.a_x = host.a_x;
        plan_start.a_y = host.a_y;
        plan_start.v_x = host.v_x;
        plan_start.v_y = host.v_y;
        // 开始规划的时间为 current_time + 100ms
        plan_start.time = cur_time + 100;
    } else {
        // 不是首次运行时，此时已经有上一周期规划的轨迹，找到当前时刻在上一规划周期应该在的位置，计算位置误差
        int desire_index = -1;
        for (int i = 0; i < pre_plan_tra.size() - 1; i++) {
            if (pre_plan_tra[i].time <= cur_time && pre_plan_tra[i+1].time > cur_time) {
                desire_index = i;
                break;
            }
        }
        if (desire_index == -1) {
            ROS_WARN_ONCE("calc_plan_start: can't find desire_index in pre_tractory!!!");
            // 把时间序列打印出来
            // std::cout << ">>>>> " << cur_time << std::endl;
            // for (int i = 0; i < pre_plan_tra.size(); i++) {
            //     std::cout << pre_plan_tra[i].time << std::endl;
            // }
            return false;
        }
        // 当前时刻期望的坐标 x y
        Vector2d pre_desire_vec(pre_plan_tra[desire_index].x, pre_plan_tra[desire_index].y);
        // 当前时刻期望的 航向角
        double pre_heading_desire = pre_plan_tra[desire_index].heading;
        // 当前小车实际坐标向量
        Vector2d host_vec(host.x, host.y);

        // this->plan_start_pub(pre_plan_tra[desire_index].x, pre_plan_tra[desire_index].y, pre_plan_tra);
        
        /// 计算纵向误差和横向误差
        Vector2d tor(cos(pre_heading_desire), sin(pre_heading_desire));     // 切向量
        Vector2d nor(-sin(pre_heading_desire), cos(pre_heading_desire));    // 法向量
        Vector2d d_err = host_vec - pre_desire_vec;                         // 误差向量
        double lon_err = abs(d_err.dot(tor));                               // 纵向误差
        double lat_err = abs(d_err.dot(nor));                               // 横向误差
        // 如果纵向误差大于 2, 横向误差大于 0.5 就认为控制没有跟上，通过运动学递推
        if (lon_err > 2 || lat_err > 0.5) {
            plan_start.x = host.x + host.v_x * dt + 0.5 * host.a_x * dt * dt;
            plan_start.y = host.y + host.v_y * dt + 0.5 * host.a_y * dt * dt;
            plan_start.v_x = host.v_x + host.a_x * dt;
            plan_start.v_y = host.v_y + host.a_y * dt;
            plan_start.heading = atan2(plan_start.v_y, plan_start.v_x);
            plan_start.kappa = host.kappa;
            // plan_start.speed = plan_start.v_x / cos(plan_start.heading);
            // plan_start.accel = host.accel;
            plan_start.a_x = host.a_x;
            plan_start.a_y = host.a_y;
            plan_start.time = cur_time + 100;
        } else {
            // 控制可以跟上规划，开始拼接，找到 cur_time + 100ms 的点
            int start_index = -1;
            for (int i = 0; i < pre_plan_tra.size() - 1; i++) {
                if (pre_plan_tra[i].time <= (cur_time + 100) && pre_plan_tra[i+1].time > (cur_time + 100)) {
                    start_index = i;
                    break;
                }
            }
            if (start_index == -1) {
                ROS_WARN_ONCE("calc_plan_start: can't find start_index in pre_tractory!!!");
                return false;
            }
            // 此算法要求轨迹的点要足够密集，不然 start_index 和 desire_index 有可能是同一个点
            plan_start.x = pre_plan_tra[start_index].x;
            plan_start.y = pre_plan_tra[start_index].y;
            plan_start.heading = pre_plan_tra[start_index].heading;
            plan_start.kappa = pre_plan_tra[start_index].kappa;
            // 规划的轨迹中的speed 表示小车在轨迹中的切向速度
            plan_start.v_x = pre_plan_tra[start_index].speed * cos(plan_start.heading);
            plan_start.v_y = pre_plan_tra[start_index].speed * sin(plan_start.heading);
            // 轨迹中的 accel 是指切向加速度，要计算 a_x a_y 需要计算切向加速度和法向加速度
            Vector2d start_tor(cos(plan_start.heading), sin(plan_start.heading));
            Vector2d start_nor(-sin(plan_start.heading), cos(plan_start.heading));
            // 切向加速度向量
            Vector2d start_a_tor = pre_plan_tra[start_index].accel * start_tor;
            // 法向加速度向量 a = v^2/r   k = 1 / r
            Vector2d start_a_nor = pow(pre_plan_tra[start_index].speed, 2) * plan_start.kappa * start_nor;
            plan_start.a_x = start_a_tor[0] + start_a_nor[0];
            plan_start.a_y = start_a_tor[1] + start_a_nor[1];
            /**
             * 此时 start_index 就是本周起规划起点的 index
             * 拼接是从 start_index 开始往前拼接 stitch_num 个，也就是 pre_plan_tra(start_index - 1), -2, -stitch_num
             * 这里分 stitch_num 小于和大于 stitch_num 两种情况
             * 还有一个点，这里拼接的轨迹不包含 plan_start 这个点
             */
            vector<TraPoint>::iterator start_stitch_ptr;
            int begin_offset = start_index - stitch_num;
            if (begin_offset >= 0) {      // 如果前面多于 start_num 个点
                // 开始截取的位置
                start_stitch_ptr = pre_plan_tra.begin() + begin_offset;
                stitch_tra.assign(start_stitch_ptr, start_stitch_ptr + stitch_num);
            }
            else {                        // 不足 start_num 个点，就从头开多少始取，能取
                start_stitch_ptr = pre_plan_tra.begin();
                stitch_tra.assign(start_stitch_ptr, start_stitch_ptr + start_index);
            }
            // std::cout << "lon_err: " << lon_err << " lat_err: " << lat_err << " stitch_num: " << stitch_tra.size() << std::endl;
        }
    }
    return true;
}


/**
 * dp[j][i]:表示从plan_start到第 j 列第 i 行的点最小的代价
 * min_point[j]：表示第j列cost最小的节点
*/
bool Planner::dp_decision(const Location &plan_start, const SLPoint &plan_start_sl,
                          const ReferencePoint &plan_start_proj, const int plan_start_m_index,
                          const vector<ReferencePoint> &cur_ref_line, 
                          const vector<SLPoint> &obs_vec_sl, vector<SLPoint> &sl_plan_tra) {
    vector<vector<double>> dp(this->dp_col_num + 1, vector<double>(this->dp_row_num));
    // 首先对于第一列（col = 0）的值赋为 0
    for (int row = 0; row < this->dp_row_num; row++)
        dp[0][row] = 0;
    vector<Location> node_list;
    // 对于 col = 1 列，分别计算从 plan_start 到该列各个点的cost 
    for (int row = 0; row < this->dp_row_num; row++) {
        SLPoint end_node = this->IJ2SL(plan_start_sl, 1, row);
        dp[1][row] = this->calculateCost(plan_start_sl, end_node, obs_vec_sl);
        // 计算当前 node 的笛卡尔坐标 xy
        // Location xy_point;
        // this->IJ2XY(plan_start_sl, plan_start_proj, plan_start_m_index, cur_ref_line, row, 1, xy_point);
        // node_list.push_back(xy_point);
    }
    // 从 col = 2 列开始进行动态规划
    for (int col_index = 2; col_index < this->dp_col_num + 1; col_index++) {
        auto min_ptr = min_element(dp[col_index - 1].begin(), dp[col_index - 1].end());
        for (int row_index = 0; row_index < this->dp_row_num; row_index++) {
            // Location xy_point;
            // this->IJ2XY(plan_start_sl, plan_start_proj, plan_start_m_index, cur_ref_line, row_index, col_index, xy_point);
            // node_list.push_back(xy_point);

            // 获取上一列cost最小的坐标
            SLPoint begin_node = this->IJ2SL(plan_start_sl, col_index - 1, min_ptr - dp[col_index - 1].begin());
            // 当前坐标
            SLPoint end_node = this->IJ2SL(plan_start_sl, col_index, row_index);
            // 当前点的cost，等于前一列最小的cost加上当前点的cost
            dp[col_index][row_index] = *min_ptr + this->calculateCost(begin_node, end_node, obs_vec_sl);
        }
    }
    // this->plan_start_pub(plan_start.x, plan_start.y, node_list);

    // 提取路径
    SLPoint pre_node = plan_start_sl;
    for (int j = 1; j < this->dp_col_num + 1; j++) {
        auto min_ptr = min_element(dp[j].begin(), dp[j].end());
        SLPoint cur_node = this->IJ2SL(plan_start_sl, j, min_ptr - dp[j].begin());
        // 根据 pre_node 和 cur_node 计算五次多项式的系数
        vector<double> params = this->calculateQuinticParams(pre_node, cur_node);
        // 对五次多项式采样 num 个SL点
        int num = delta_s / interval;
        ArrayXd ds(num);    
        ArrayXd l(num);
        ArrayXd dl(num);
        ArrayXd ddl(num);
        for (int i = 0; i < num; i++) {
            ds(i) = pre_node.s + i * delta_s / num;
        }
        l = params[0] * Eigen::ArrayXd::Ones(num) + params[1] * ds + 
            params[2] * ds.pow(2) + params[3] * ds.pow(3) + 
            params[4] * ds.pow(4) + params[5] * ds.pow(5);
        dl = params[1] * ArrayXd::Ones(num) + 2 * params[2] * ds + 
             3 * params[3] * ds.pow(2) + 4 * params[4] * ds.pow(3) + 
             5 * params[5] * ds.pow(4);
        ddl = 2 * params[2] * ArrayXd::Ones(num) + 
              6 * params[3] * ds + 12 * params[4] * ds.pow(2) + 
              20 * params[5] * ds.pow(3);
        // 对每一个点计算其笛卡尔点
        for (int index = 0; index < num; index++) {
            Location xy_point;
            SLPoint new_sl;
            new_sl.s = ds[index];
            new_sl.l = l[index];
            new_sl.l_prime = dl[index];
            new_sl.l_dprime = ddl[index];
            // int interval_offset = (j - 1) * num + index;
            sl_plan_tra.push_back(new_sl);
        }
        pre_node = cur_node;
    }
    return true;
}


/**
 * 将规划输出的sl轨迹转化为全局坐标轨迹
*/
bool Planner::traSL2XY(const ReferencePoint &plan_start_proj, const int plan_start_m_index, 
                       const vector<ReferencePoint> cur_ref_line, 
                       const vector<SLPoint> &plan_tra_sl, vector<Location> &path_xy) {
    for (int i = 0; i < plan_tra_sl.size(); i++) {
        Location xy_point;
        this->IJ2XY(plan_start_proj, plan_start_m_index, cur_ref_line, i, plan_tra_sl[i] ,xy_point);
        path_xy.push_back(xy_point);
    }
    return true;
}


/**
 * 将第 j 列第 i 行的点转化为 SL自然坐标系中的坐标
*/
SLPoint Planner::IJ2SL(const SLPoint &plan_start_sl, int cur_col, int cur_row) {
    SLPoint sl_point;
    sl_point.s = plan_start_sl.s + delta_s * cur_col;
    sl_point.l = (this->dp_row_num / 2 - cur_row) * delta_l;
    return sl_point;
}


/**
 * 以plan_start 为起点，根据 interval_offset 计算投影点，然后计算笛卡尔坐标
 * @param plan_start_proj: 开始位置的投影点
 * @param plan_start_m_index: 开始位置匹配点的下标
 * @param cur_ref_line: 当前参考线
 * @param interval_offset: 要求点的偏移量（以interval为单位）
 * @param new_sl: 要转换的SL坐标
 * @param xy_point: 要求的笛卡尔坐标
*/
bool Planner::IJ2XY(const ReferencePoint &plan_start_proj, 
                    const int plan_start_m_index, const vector<ReferencePoint> cur_ref_line, 
                    const int interval_offset, const SLPoint &new_sl, Location &xy_point) {
    int m_index_new = plan_start_m_index + interval_offset;
    // 计算 plan_start_proj 与 plan_start_m_index 的距离 dis
    double dis = sqrt(pow(cur_ref_line[plan_start_m_index].x - plan_start_proj.x, 2) + 
                      pow(cur_ref_line[plan_start_m_index].y - plan_start_proj.y, 2));
    // dis 和 要求的投影点与 m_index_new 的距离是一样的
    ReferencePoint m_point_new = cur_ref_line[m_index_new];
    ReferencePoint IJ_proj;       // 要求的的投影点
    IJ_proj.x = m_point_new.x + dis * cos(m_point_new.heading);
    IJ_proj.y = m_point_new.y + dis * sin(m_point_new.heading);
    IJ_proj.heading = m_point_new.heading + dis * m_point_new.kappa;
    IJ_proj.kappa = m_point_new.kappa;
    IJ_proj.dkappa = 0;
    IJ_proj.s = plan_start_proj.s + interval_offset * interval;
    // 将 new_sl 转化到笛卡尔坐标系
    // xy_point.x = IJ_proj.x - sin(IJ_proj.heading) * new_sl.l;
    // xy_point.y = IJ_proj.y + cos(IJ_proj.heading) * new_sl.l;
    this->referenceline_provider.frenet2Cartesian(IJ_proj, new_sl, xy_point);
    return true;
}


/**
 * 将动态规划中的第j列第i行的点转化为世界坐标
 * 步骤1： 计算第j列第i行的点的投影点
 *      方法：根据plan_start 在参考线的投影 plan_start_proj 和 plan_start_m_index 计算(i,j) 对应的匹配点和投影点
 *      在自然坐标系中，delta_s 的长度和两个参考线点之间距离 interval 的关系是一个整数倍
 *      例如：j = 1, delta_s = 10, interval = 0.5, plan_start_proj 的 plan_start_m_index = 15
 *      那么(i, j) 对应投影点的匹配点下标为 plan_start_m_index + j * delta_s / interval = 15 + 1 * 10 / 0.5 = 35
 *      有了(i, j) 对应投影点的匹配点下标， 就可以计算投影点的信息
 * 步骤2： 根据投影点计算笛卡尔坐标点
*/
bool Planner::IJ2XY(const SLPoint &plan_start_sl,
                    const ReferencePoint &plan_start_proj, const int plan_start_m_index, 
                    const vector<ReferencePoint> cur_ref_line, 
                    const int i, const int j, Location &xy_point) {
    // 计算(i, j)投影点的匹配点
    int m_index_ij = plan_start_m_index + j * delta_s / interval;
    // 计算 plan_start_proj 与 plan_start_m_index 的距离 dis
    double dis = sqrt(pow(cur_ref_line[plan_start_m_index].x - plan_start_proj.x, 2) + 
                      pow(cur_ref_line[plan_start_m_index].y - plan_start_proj.y, 2));
    // dis 和 要求的投影点与 m_index_ij 的距离是一样的
    ReferencePoint m_ij_point = cur_ref_line[m_index_ij];
    ReferencePoint IJ_proj;       // (i, j) 对应的投影点
    IJ_proj.x = m_ij_point.x + dis * cos(m_ij_point.heading);
    IJ_proj.y = m_ij_point.y + dis * sin(m_ij_point.heading);
    IJ_proj.heading = m_ij_point.heading + dis * m_ij_point.kappa;
    IJ_proj.kappa = m_ij_point.kappa;
    IJ_proj.dkappa = 0;
    // (i, j)的投影点的 s 等于plan_start 投影点的 s 加上 偏移的距离 delta_s * j
    IJ_proj.s = plan_start_proj.s + delta_s * j;
    SLPoint IJ_sl = this->IJ2SL(plan_start_sl, j, i);
    // 将自然坐标系的点 IJ_sl 转化到笛卡尔坐标
    // this->referenceline_provider.frenet2Cartesian(IJ_proj, IJ_sl, xy_point);
    xy_point.x = IJ_proj.x - sin(IJ_proj.heading) * IJ_sl.l;
    xy_point.y = IJ_proj.y + cos(IJ_proj.heading) * IJ_sl.l;
    return true;
}


double Planner::calculateCost(const SLPoint &start_sl, const SLPoint &end_sl, const vector<SLPoint> &obs_vec_sl)
{   
    // 计算五次多项式参数
    vector<double> params = this->calculateQuinticParams(start_sl, end_sl);
    double a0 = params[0];
    double a1 = params[1];
    double a2 = params[2];
    double a3 = params[3];
    double a4 = params[4];
    double a5 = params[5];
    // 在这段曲线上取10个点，计算cost
    int size_num = 5;
    Eigen::ArrayXd ds(size_num);
    Eigen::ArrayXd l(size_num);
    Eigen::ArrayXd dl(size_num);
    Eigen::ArrayXd ddl(size_num);
    Eigen::ArrayXd dddl(size_num);
    for (int i = 0; i < size_num; i++) {
        ds(i) = start_sl.s + i * delta_s / size_num;
    }
    l = a0 * Eigen::ArrayXd::Ones(size_num) + a1 * ds + a2 * ds.pow(2) + a3 * ds.pow(3) + a4 * ds.pow(4) + a5 * ds.pow(5);
    dl = a1 * ArrayXd::Ones(size_num) + 2 * a2 * ds + 3 * a3 * ds.pow(2) + 4 * a4 * ds.pow(3) + 5 * a5 * ds.pow(4);
    ddl = 2 * a2 * ArrayXd::Ones(size_num) + 6 * a3 * ds + 12 * a4 * ds.pow(2) + 20 * a5 * ds.pow(3);
    dddl = 6 * a3 * ArrayXd::Ones(size_num) + 24 * a4 * ds + 60 * a5 * ds.pow(2);

    double cost_smooth = W_cost_smooth_dl * (dl.matrix().transpose().dot(dl.matrix())) + 
                         W_cost_smooth_ddl * (ddl.matrix().transpose().dot(ddl.matrix())) +
                         W_cost_smooth_dddl * (dddl.matrix().transpose().dot(dddl.matrix()));
    double cost_ref = W_cost_ref * (l.matrix().transpose().dot(l.matrix()));

    // Eigen::ArrayXd l_abs = l.abs();
    // Eigen::ArrayXd dl_abs = dl.abs();
    // Eigen::ArrayXd ddl_abs = ddl.abs();
    // Eigen::ArrayXd dddl_abs = dddl.abs();
    // double cost_smooth = W_cost_smooth_dl * dl_abs.sum() + 
    //                      W_cost_smooth_ddl * ddl_abs.sum() + 
    //                      W_cost_smooth_dddl * dddl_abs.sum();
    // double cost_ref = W_cost_ref * l_abs.sum();
    double cost_collision = 0;
    ArrayXd square_d;
    for (int i = 0; i < obs_vec_sl.size(); i++) {
        ArrayXd dlon = ArrayXd::Ones(size_num) * obs_vec_sl[i].s - ds;
        ArrayXd dlat = ArrayXd::Ones(size_num) * obs_vec_sl[i].l - l;
        square_d = dlon.pow(2) + dlat.pow(2);
        cost_collision += this->calcObsCost(W_cost_collision, square_d);
    }
    // std::cout << square_d.transpose() << std::endl;
    double tatol_cost = cost_smooth + cost_ref + cost_collision;
    return tatol_cost;
}


/**
 * 计算障碍物的距离代价
*/
double Planner::calcObsCost(double W_cost_collision, Eigen::ArrayXd &square_d)
{
    double obs_cost = 0;
    for (int i = 0; i < square_d.rows(); i++) {
        if (square_d(i) < 100 && square_d(i) > 16)
            obs_cost += (W_alpha_collision / square_d(i));
        else
            obs_cost += W_cost_collision;
    }
    return obs_cost;
}


vector<double> Planner::calculateQuinticParams(const SLPoint &start_sl, const SLPoint &end_sl)
{
    double start_s = start_sl.s;
    double end_s = end_sl.s; 
    
    double start_s2 = pow(start_s, 2);
    double start_s3 = pow(start_s, 3);
    double start_s4 = pow(start_s, 4);
    double start_s5 = pow(start_s, 5);
    double end_s2 = pow(end_s, 2);
    double end_s3 = pow(end_s, 3);
    double end_s4 = pow(end_s, 4);
    double end_s5 = pow(end_s, 5);
    Eigen::MatrixXd A(6, 6);
    A << 1,    start_s,    start_s2,    start_s3,   start_s4,    start_s5,
         0,    1,          2*start_s,   3*start_s2, 4*start_s3,  5*start_s4,
         0,    0,          2,           6*start_s,  12*start_s2, 20*start_s3,
         1,    end_s,      end_s2,      end_s3,     end_s4,      end_s5,
         0,    1,          2*end_s,     3*end_s2,   4*end_s3,    5*end_s4,
         0,    0,          2,           6*end_s,    12*end_s2,   20*end_s3;
    Eigen::MatrixXd B(6, 1);
    B << start_sl.l, 
         start_sl.l_prime,
         start_sl.l_dprime,
         end_sl.l, 
         end_sl.l_prime,
         end_sl.l_dprime;
    Eigen::VectorXd X = A.inverse() * B;

    vector<double> res;
    for (int i = 0; i < X.rows(); i++)
        res.push_back(X[i]);
    return res;
}


/**
 * 用 POINTS 展示参考线的点
 * 用 LINE_LIST 表示当前期望的车辆位置
*/
void Planner::plan_start_pub(double x, double y, vector<Location> &list_point) {

    visualization_msgs::Marker line_list, points;
    
    line_list.header.frame_id = points.header.frame_id = "world";
    line_list.header.stamp = points.header.stamp = ros::Time::now();
    line_list.ns = points.ns = "debug";
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    points.type = visualization_msgs::Marker::POINTS;

    line_list.action = points.action = visualization_msgs::Marker::ADD;
    line_list.id = 0;
    points.id = 1;

    points.scale.x = 0.2;
    points.scale.y = 0.2;
    line_list.scale.x = 0.3;

    line_list.color.r = 1.0;
    line_list.color.a = 1.0;

    points.color.r = 1.0;
    points.color.a = 1.0;

    // 将轨迹点添加进 point
    for (int i = 0; i < list_point.size(); i++) {
        geometry_msgs::Point p;
        p.x = list_point[i].x;
        p.y = list_point[i].y;
        p.z = 0.0;
        points.points.push_back(p);
    }
    // 将要展示的点添加进 line_list
    geometry_msgs::Point p;
    p.x = x;
    p.y = y;
    p.z = 0;
    line_list.points.push_back(p);
    p.z += 3;
    line_list.points.push_back(p);

    // line_list.lifetime = ros::Duration();
    this->debug_pub_.publish(line_list);
    this->debug_pub_.publish(points);
}


void Planner::trajectory_pub_fun() {
    ros::Rate rate(10);
    while (ros::ok())
    {   
        std::string frame_id = "world";
        ros::Time cur_time = ros::Time::now();
        // ------------- trajectory marker ----------------
        visualization_msgs::Marker line_strip;
        line_strip.header.frame_id = frame_id;
        line_strip.header.stamp = cur_time;
        line_strip.ns = "show_planner";
        line_strip.action = visualization_msgs::Marker::ADD;
        line_strip.pose.orientation.w = 1.0;

        line_strip.id = 1;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        line_strip.scale.x = 1.0;   // 线条的宽度

        line_strip.color.g = 1.0;
        line_strip.color.a = 0.5;

        // ------------- trajectory path ----------------
        nav_msgs::Path plan_tra_path;
        plan_tra_path.header.frame_id = frame_id;
        plan_tra_path.header.stamp = cur_time;
        plan_tra_path.poses.clear();


        // vector<ReferencePoint> ref_line = this->referenceline_provider.getReferenceLine();
        vector<TraPoint> tra(this->trajectory);
        for (int i = 0; i < tra.size(); i++) {
            // ------------- trajectory marker ----------------
            geometry_msgs::Point point;
            point.x = tra[i].x;
            point.y = tra[i].y;
            point.z = 0;
            line_strip.points.push_back(point);

            // ------------- trajectory path ----------------
            geometry_msgs::PoseStamped pose;
            pose.header.frame_id = frame_id;
            pose.header.stamp = cur_time;
            pose.pose.position.x = tra[i].x;
            pose.pose.position.y = tra[i].y;
            pose.pose.position.z = 0;

            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 0.0;
            plan_tra_path.poses.push_back(pose);
        }
        this->trajectory_pub.publish(plan_tra_path);
        this->trajectory_marker_pub.publish(line_strip);

        rate.sleep();
    }
}

/**
 * 发布line_strip 类型的 marker 数据
*/
void Planner::marker_line_strip_pub_fun(const vector<TraPoint> &list, const string ns, 
                                const int32_t id, const Color color) {
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "world";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = ns;
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id;

    line_strip.scale.x = 0.5;
    line_strip.color.r = color.r;
    line_strip.color.g = color.g;
    line_strip.color.b = color.b;
    line_strip.color.a = color.a;

    for (int i = 0; i < list.size(); i++) {
        geometry_msgs::Point point;
        point.x = list[i].x;
        point.y = list[i].y;
        point.z = 3.0;
        line_strip.points.push_back(point);
    }
    this->debug_pub_.publish(line_strip);
}


void Planner::run()
{   
    // 启动参考线模块
    this->referenceline_provider.run();
    this->host_info.run();
    this->obs_info.run();
    
    this->trajectory_pub = nh.advertise<nav_msgs::Path>("/trajectory_plan", 10);
    this->trajectory_marker_pub = nh.advertise<visualization_msgs::Marker>("/trajectory_marker", 10);
    this->debug_pub_ = nh.advertise<visualization_msgs::Marker>("/trajectory_marker", 10);
    this->speed_planner_info_pub = nh.advertise<em_planner::SpeedPlannerInfo>("/speed_planner_info", 10);

    boost::thread pub_ref_line_thread(boost::bind(&Planner::trajectory_pub_fun, this));

    ros::Rate sub_rate(10);     // 以10hz的频率接收订阅消息
    while (ros::ok()) {
        ros::spinOnce();

        this->pre_trajectory.assign(this->trajectory.begin(), this->trajectory.end());
        vector<TraPoint> tra;
        clock_t time_start, time_end;
        time_start = clock();
        // 开始规划流程，生成运动轨迹
        if (!this->runPlanner(tra)) {
            continue;
        }
        time_end = clock();
        std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;
        this->trajectory.assign(tra.begin(), tra.end());

        
        sub_rate.sleep();
    }

}